#ifndef _MOTOR_H_
#define _MOTOR_H_
#define RX_GPIO_PIN GPIO_NUM_5
#define TX_GPIO_PIN GPIO_NUM_4


enum{
    MSG_CO_NMT_CTRL = 0X000,
    MSG_ODRIVE_HEARTBEAT,
    MSG_ODRIVE_ESTOP,
    MSG_GET_MOTOR_ERROR,
    MSG_GET_ENCODER_ERROR,
    MSG_GET_SENSORLESS_ERROR,
    MSG_SET_AXIS_NODE_ID,
    MSG_SET_AXIS_REQUESTED_STATE,
    MSG_SET_AXIS_STARTUP_CONFIG,
    MSG_GET_ENCODER_ESTIMATES,
    MSG_GET_ENCODER_COUNT,
    MSG_SET_CONTROLLER_MODES,
    MSG_SET_INPUT_POS,
    MSG_SET_INPUT_VEL,
    MSG_SET_INPUT_TORQUE,
    MSG_SET_VEL_LIMIT,
    MSG_START_ANTICOGGING,
    MSG_SET_TRAJ_VEL_LIMIT,
    MSG_SET_TRAJ_ACCEL_LIMITS,
    MSG_SET_TRAJ_INERTIA,
    MSG_GET_IQ,
    MSG_GET_SENSORLESS_ESTIMATES,
    MSG_RESET_ODRIVE,
    MSG_GET_VBUS_VOLTAGE,
    MSG_CLEAR_ERRORS,
    MSG_CO_HEARTBEAT_CMD = 0X700,
};

typedef struct Motor
{
    uint8_t CanConnectState;
    uint16_t NodeID;
    double Position;
    double Speed;
    int16_t Acceleration;
}Motor;
extern Motor motor1;
extern Motor motor2;

void ResetMotortWatchDog(uint8_t MotorIndex);
void SetMotortWatchDogReady(uint8_t MotorIndex);
uint8_t GetWatchDogState(uint8_t MotorIndex);


void MotorInit(void);
void SetMotorTorque(uint8_t motorIndex,float torque);
void SetMotorLimit(uint8_t motorIndex,float velLimit,float currentSoftMax);

uint8_t CanInit(void);
void CanSendMessage(uint8_t node_id,uint8_t cmd, uint8_t *data, uint8_t len);
void CanRecvMessage(void);
double GetMotorPos(uint8_t motorIndex);
double GetMotorSpeed(uint8_t motorIndex);
void ResetEncoder();
#endif
